#!/usr/bin/env python3
# encoding: utf-8
# @Author: Zecous
# @Date: 2025/07/18
# 主比赛程序 - 协调所有任务执行

import cv2
import numpy as np
import rospy
import signal
import time
import threading
import os
from std_msgs.msg import String, Bool
from std_srvs.srv import Trigger, TriggerRequest
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from ainex_sdk import misc, common
from ainex_example.color_common import Common

class CompetitionController(Common):
    def __init__(self, name):
        # 初始化头部位置
        self.head_pan_init = 500
        self.head_tilt_look_down = 280
        
        # 初始化ROS节点
        rospy.init_node(name)
        self.name = name
        self.running = True
        
        super().__init__(name, self.head_pan_init, self.head_tilt_look_down)
        
        # 设置退出处理函数
        signal.signal(signal.SIGINT, self.shutdown)
        
        # Initialize CV Bridge
        self.bridge = CvBridge()
        
        # 加载配置文件
        self.load_config()
        
        # 竞赛状态管理
        self.current_state = "forward_start"  # 当前状态，先前进
        self.competition_finished = False
        
        # 初始前进参数
        self.forward_step_count = 0
        self.max_forward_steps = 5
        
        # 任务完成状态
        self.task_completed = {
            "red_object": False,    # 红色物体绕行
            "blue_object": False,   # 蓝色物体跨栏
            "green_object": False,  # 绿色物体台阶
            "purple_object": False  # 紫色物体投掷
        }
        
        # 任务执行顺序
        self.task_sequence = [
            ("go_around", "red"),      # 绕行任务 - 检测红色物体
            ("hurdles", "blue"),       # 跨栏任务 - 检测蓝色物体  
            ("stairs", "green"),       # 台阶任务 - 检测绿色物体
            ("throw", "purple")        # 投掷任务 - 检测紫色物体
        ]
        
        # 当前任务索引
        self.current_task_index = 0
        
        # 图像和检测相关
        self.current_image = None
        self.image_width = self.config.get('camera', {}).get('width', 640)
        self.image_height = self.config.get('camera', {}).get('height', 480)
        
        # 任务进度信息
        self.progress_info = {
            "current_task": "Start Competition",
            "progress": "Initializing...",
            "detected_objects": [],
            "completed_tasks": []
        }
        
        # 物体检测稳定性检查
        self.detection_history = {}
        self.detection_stability_frames = self.config.get('image_processing', {}).get('detection', {}).get('stability_frames', 8)
        # 对红色物体使用更多稳定性帧数（防止误触发）
        self.red_stability_frames = 5  # 红色物体需要更多稳定帧数
        
        # 从配置加载红色物体触发条件
        go_around_detection = self.config.get('tasks', {}).get('go_around', {}).get('detection', {})
        self.red_min_area = go_around_detection.get('min_area', 3000)  # 红色物体最小面积
        self.red_min_y_ratio = go_around_detection.get('roi', {}).get('y_min', 0.55)  # 红色物体应该在视野的下方
        
        # 目标线参数 - 在load_config()中设置
        
        # 线程锁
        self.state_lock = threading.Lock()
        
        # 在设置通信之前就立即停止循线节点
        print("Immediately stopping line following at startup...")
        try:
            # 等待ROS服务可用并停止循线
            rospy.wait_for_service('/line_follow/stop', timeout=5.0)
            service_proxy = rospy.ServiceProxy('/line_follow/stop', Trigger)
            response = service_proxy(TriggerRequest())
            print(f"Early line following stop result: {response.success}")
        except Exception as e:
            print(f"Early line following stop failed: {e}")
        
        # ROS通信设置
        self.setup_ros_communication()
        
        # Subscribe to camera topic
        camera_topic = self.config.get('ros', {}).get('topics', {}).get('camera_image', '/camera/image_raw')
        self.image_subscriber = rospy.Subscriber(camera_topic, Image, self.image_callback)
        
        # 初始化机器人
        self.motion_manager.run_action('walk_ready')
        self.set_servo(self.head_pan_init, self.head_tilt_look_down)
        time.sleep(2)
        
        # 初始化运动参数（参考循线节点）
        self.init_movement_params()
        
        # 再次确保循线节点已停止
        print("Double-checking line following is stopped...")
        try:
            self.call_service('/line_follow/stop', timeout=2.0)
        except:
            pass
        
        common.loginfo('Competition controller initialized')
        print("Competition started! All task nodes ready...")
        print(f"[DEBUG] 初始化完成 - image_height: {self.image_height}, image_width: {self.image_width}")
        print(f"[DEBUG] target_line_y_ratio: {self.target_line_y_ratio}, 计算的目标线Y: {int(self.target_line_y_ratio * self.image_height)}")

    def load_config(self):
        """加载配置文件"""
        try:
            # 获取功能包路径
            package_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
            config_path = os.path.join(package_path, 'config', 'competition_config.yaml')
            
            if os.path.exists(config_path):
                self.config = common.get_yaml_data(config_path)
                print(f"配置文件加载成功: {config_path}")
            else:
                print(f"配置文件不存在: {config_path}")
                self.config = {}
            
            # 从配置文件加载颜色检测范围
            self.color_ranges = {}
            
            # 首先尝试从外部配置文件加载颜色阈值
            color_config = self.config.get('image_processing', {}).get('color_config', {})
            use_external_config = color_config.get('use_external_config', False)
            external_config_file = color_config.get('external_config_file', '')
            config_section = color_config.get('config_section', 'Mono')
            
            if use_external_config and external_config_file and os.path.exists(external_config_file):
                try:
                    print(f"尝试从外部配置文件加载颜色阈值: {external_config_file}")
                    external_config = common.get_yaml_data(external_config_file)
                    
                    if 'lab' in external_config and config_section in external_config['lab']:
                        lab_config = external_config['lab'][config_section]
                        
                        # 映射颜色名称（外部配置文件使用中文，内部使用英文）
                        color_mapping = {
                            'red': 'red',
                            'blue': 'blue', 
                            'green': 'green',
                            'deep_green': 'deep_green',  # 添加深绿色映射
                            'purple': 'purple',
                            'white': 'white'
                        }
                        
                        for internal_name, external_name in color_mapping.items():
                            if external_name in lab_config:
                                color_data = lab_config[external_name]
                                if 'min' in color_data and 'max' in color_data:
                                    self.color_ranges[internal_name] = {
                                        "lower": np.array(color_data['min']),
                                        "upper": np.array(color_data['max'])
                                    }
                        
                        # 检查是否加载了深绿色配置
                        if 'deep_green' not in self.color_ranges:
                            print("没有从配置文件加载深绿色")
                            # 尝试使用绿色配置作为深绿色的基础，并调整参数
                            if 'green' in self.color_ranges:
                                green_lower = self.color_ranges['green']['lower'].copy()
                                green_upper = self.color_ranges['green']['upper'].copy()
                                # 调整深绿色的范围（通常深绿色的 L 值更低，a 值更负）
                                deep_green_lower = green_lower.copy()
                                deep_green_upper = green_upper.copy()
                                deep_green_lower[0] = max(0, deep_green_lower[0] - 20)  # L 值降低
                                deep_green_lower[1] = max(0, deep_green_lower[1] - 10)  # a 值调整
                                deep_green_upper[0] = min(255, deep_green_upper[0] - 10)  # L 值上限降低
                                self.color_ranges['deep_green'] = {
                                    "lower": deep_green_lower,
                                    "upper": deep_green_upper
                                }
                                print(f"基于绿色配置生成深绿色范围: {deep_green_lower} ~ {deep_green_upper}")
                            else:
                                # 使用默认深绿色配置
                                self.color_ranges['deep_green'] = {
                                    "lower": np.array([29, 36, 23]),
                                    "upper": np.array([198, 88, 167])
                                }
                                print("使用默认深绿色配置: [29, 36, 23] ~ [198, 88, 167]")
                        
                        if self.color_ranges:
                            print(f"从外部配置文件加载了 {len(self.color_ranges)} 种颜色阈值")
                        else:
                            print("外部配置文件中未找到有效的颜色配置")
                            
                except Exception as e:
                    print(f"从外部配置文件加载颜色阈值失败: {e}")
            
            # 如果外部配置加载失败，尝试从主配置文件加载
            if not self.color_ranges:
                print("尝试从主配置文件加载颜色阈值")
                colors_config = self.config.get('image_processing', {}).get('colors', {})
                
                for color_name, color_config in colors_config.items():
                    if 'lower' in color_config and 'upper' in color_config:
                        self.color_ranges[color_name] = {
                            "lower": np.array(color_config['lower']),
                            "upper": np.array(color_config['upper'])
                        }
            
            # 如果都没有加载到颜色范围，使用默认值
            if not self.color_ranges:
                print("使用默认颜色检测范围")
                self.color_ranges = {
                    "red": {"lower": np.array([44, 138, 130]), "upper": np.array([163, 196, 255])},
                    "blue": {"lower": np.array([0, 94, 0]), "upper": np.array([243, 179, 113])},
                    "green": {"lower": np.array([136, 0, 155]), "upper": np.array([255, 255, 255])},
                    "purple": {"lower": np.array([20, 114, 82]), "upper": np.array([105, 157, 122])}
                }
            
            # 加载其他配置参数
            self.min_area = self.config.get('image_processing', {}).get('detection', {}).get('min_area', 500)
            self.position_tolerance = self.config.get('image_processing', {}).get('detection', {}).get('position_tolerance', 50)
            self.horizontal_threshold = self.config.get('image_processing', {}).get('detection', {}).get('horizontal_threshold', 10)
            self.detection_stability_frames = self.config.get('image_processing', {}).get('detection', {}).get('stability_frames', 5)
            
            # 加载舵机配置
            servo_config = self.config.get('servo', {})
            self.head_pan_init = servo_config.get('head_pan_init', 500)
            self.head_tilt_look_down = servo_config.get('head_tilt_look_down', 280)
            self.head_tilt_look_up = servo_config.get('head_tilt_look_up', 500)
            self.head_pan_id = servo_config.get('head_pan_id', 23)
            self.head_tilt_id = servo_config.get('head_tilt_id', 24)
            self.servo_time = servo_config.get('servo_time', 200)
            
            # 加载运动参数
            movement_config = self.config.get('movement', {})
            self.gait_params = movement_config.get('gait', {})
            self.dsp_params = movement_config.get('dsp', [400, 0.2, 0.025])
            self.arm_swap = movement_config.get('arm_swap', 30)
            self.step_distance = movement_config.get('step_distance', 0.008)
            self.adjust_step_distance = movement_config.get('adjust_step_distance', 0.005)
            self.turn_dsp_params = movement_config.get('turn_dsp', [400, 0.2, 0.023])
            self.turn_arm_swap = movement_config.get('turn_arm_swap', 30)
            self.max_yaw = movement_config.get('max_yaw', 8)
            
            # 加载任务配置
            tasks_config = self.config.get('tasks', {})
            self.task_configs = {}
            for task_name, task_config in tasks_config.items():
                if task_config.get('enabled', False):
                    self.task_configs[task_name] = task_config
            
            # 加载系统配置
            system_config = self.config.get('system', {})
            self.debug_mode = system_config.get('debug', True)
            self.log_level = system_config.get('log_level', 'INFO')
            self.competition_timeout = system_config.get('competition_timeout', 1800)
            self.task_timeout = system_config.get('task_timeout', 300)
            
            # 加载显示配置
            display_config = system_config.get('display', {})
            self.show_main_window = display_config.get('show_main_window', False)
            self.show_detection_masks = display_config.get('show_detection_masks', False)
            self.show_progress_info = display_config.get('show_progress_info', False)
            self.window_scale = display_config.get('window_scale', 1.0)
            
            # 加载安全配置
            safety_config = self.config.get('safety', {})
            self.emergency_stop_enabled = safety_config.get('emergency_stop_enabled', True)
            self.max_continuous_run_time = safety_config.get('max_continuous_run_time', 3600)
            self.auto_shutdown_on_error = safety_config.get('auto_shutdown_on_error', False)
            
            # 加载检测区域限制
            roi_config = safety_config.get('detection_roi', {})
            self.roi_x_min = roi_config.get('x_min', 0.1)
            self.roi_x_max = roi_config.get('x_max', 0.9)
            self.roi_y_min = roi_config.get('y_min', 0.1)
            self.roi_y_max = roi_config.get('y_max', 0.9)
            
            # 加载目标线参数
            target_line_config = safety_config.get('target_line', {})
            self.target_line_y_ratio = target_line_config.get('y_ratio', 0.6)  # 目标线Y位置比例（画面高度的60%）
            
            print(f"配置加载 - target_line_config: {target_line_config}")
            print(f"配置加载 - target_line_y_ratio: {self.target_line_y_ratio}")
            print(f"最终颜色配置: {list(self.color_ranges.keys())}颜色阈值已加载完成")
            print(f"启用任务: {list(self.task_configs.keys())}")
            
            # 将颜色配置发布到ROS参数服务器，供其他节点使用
            self.publish_color_config_to_ros()
            
        except Exception as e:
            print(f"配置文件加载失败: {e}")
            # 使用默认配置
            self.config = {}
            self.color_ranges = {
                "red": {"lower": np.array([44, 138, 130]), "upper": np.array([163, 196, 255])},
                "blue": {"lower": np.array([0, 94, 0]), "upper": np.array([243, 179, 113])},
                "green": {"lower": np.array([136, 0, 155]), "upper": np.array([255, 255, 255])},
                "deep_green": {"lower": np.array([29, 36, 23]), "upper": np.array([198, 88, 167])},
                "purple": {"lower": np.array([20, 114, 82]), "upper": np.array([105, 157, 122])}
            }
            self.min_area = 500
            self.position_tolerance = 50
            self.horizontal_threshold = 10
            self.detection_stability_frames = 5
            
            # 默认舵机配置
            self.head_pan_init = 500
            self.head_tilt_look_down = 280
            self.head_tilt_look_up = 500
            self.head_pan_id = 23
            self.head_tilt_id = 24
            self.servo_time = 200
            
            # 默认任务配置
            self.task_configs = {}
            
            # 默认目标线参数
            self.target_line_y_ratio = 0.6

    def publish_color_config_to_ros(self):
        """将颜色配置发布到ROS参数服务器"""
        try:
            # 发布颜色配置到ROS参数服务器
            for color_name, color_range in self.color_ranges.items():
                param_name = f'/image_processing/colors/{color_name}'
                color_config = {
                    'lower': color_range['lower'].tolist(),
                    'upper': color_range['upper'].tolist()
                }
                rospy.set_param(param_name, color_config)
            
            # 发布检测参数
            detection_config = {
                'min_area': self.min_area,
                'stability_frames': self.detection_stability_frames,
                'position_tolerance': self.position_tolerance,
                'horizontal_threshold': self.horizontal_threshold
            }
            rospy.set_param('/image_processing/detection', detection_config)
            
            # 发布舵机配置
            servo_config = {
                'head_pan_init': self.head_pan_init,
                'head_tilt_look_down': self.head_tilt_look_down,
                'head_tilt_look_up': self.head_tilt_look_up,
                'head_pan_id': self.head_pan_id,
                'head_tilt_id': self.head_tilt_id,
                'servo_time': self.servo_time
            }
            rospy.set_param('/servo', servo_config)
            
            # 发布任务配置
            for task_name, task_config in self.task_configs.items():
                param_name = f'/tasks/{task_name}'
                rospy.set_param(param_name, task_config)
            
            print("颜色配置已发布到ROS参数服务器")
            
        except Exception as e:
            print(f"发布配置到ROS参数服务器失败: {e}")

    def init_movement_params(self):
        """初始化运动参数（参考循线节点）"""
        try:
            self.calib_config = common.get_yaml_data('/home/ubuntu/ros_ws/src/ainex_example/config/calib.yaml')
        except:
            self.calib_config = {}
        
        # 获取步态参数
        self.walking_param = self.gait_manager.get_gait_param()
        self.walking_param['step_height'] = 0.02    # 步高
        self.walking_param['pelvis_offset'] = 5      # 盆骨偏移
        self.walking_param['hip_pitch_offset'] = 15  # 髋关节偏移
        self.walking_param['body_height'] = 0.035    # 身体高度
        self.walking_param['init_y_offset'] = 0.000    # 初始y偏移
        
        # 运动参数设置（参考循线节点）
        self.dsp_param = [350, 0.2, 0.025]  # DSP参数
        self.arm_swap = 60                   # 摆臂幅度
        self.step_distance = 0.012          # 前进步长
        
        print(f"Movement parameters initialized: step_distance={self.step_distance}, dsp_param={self.dsp_param}")

    def execute_forward_steps(self):
        """执行固定前进步数"""
        print(f"Starting forward walk: {self.max_forward_steps} steps")
        self.progress_info["current_task"] = "Initial Forward Walk"
        self.progress_info["progress"] = f"Walking forward {self.forward_step_count}/{self.max_forward_steps} steps..."
        
        # 强制停止循线和步态管理器，完全接管控制权
        print("Taking full control of gait manager for forward walk...")
        try:
            # 停止循线节点
            self.call_service('/line_follow/stop', timeout=1.0)
            # 停止步态管理器
            self.gait_manager.stop()
            time.sleep(1.0)  # 给更多时间让停止命令生效
        except Exception as e:
            print(f"Error stopping services: {e}")
        
        # 发布状态表明我们正在执行前进
        self.publish_competition_status("executing_forward_walk")
        
        for step in range(self.max_forward_steps):
            if not self.running:
                break
                
            try:
                print(f"Executing forward step {step + 1}/{self.max_forward_steps}")
                
                # 执行前进步态
                self.gait_manager.set_step(
                    self.dsp_param,        # DSP参数
                    self.step_distance,    # x方向步长（前进）
                    0,                     # y方向步长（不侧移）
                    0,                     # yaw角度（不转向）
                    self.walking_param,    # 步态参数
                    self.arm_swap,         # 摆臂幅度
                    0                      # 连续移动
                )
                
                self.forward_step_count += 1
                self.progress_info["progress"] = f"Walking forward {self.forward_step_count}/{self.max_forward_steps} steps..."
                
                # 等待步态完成
                time.sleep(0.8)  # 根据实际步态周期调整
                
            except Exception as e:
                print(f"Error during forward step {step + 1}: {e}")
                rospy.logerr(f"Forward step error: {e}")
        
        print(f"Forward walk completed: {self.forward_step_count} steps")
        
        # 停止步态管理器，准备交给循线节点控制
        try:
            self.gait_manager.stop()
            time.sleep(0.5)
        except:
            pass
            
        self.publish_competition_status("forward_start_completed")

    def setup_ros_communication(self):
        """设置ROS通信"""
        # 状态订阅器
        self.task_status_subs = {
            'line_following': rospy.Subscriber('/line_follow/status', String, 
                                             lambda msg: self.task_status_callback('line_following', msg)),
            'go_around': rospy.Subscriber('/go_around/status', String, 
                                        lambda msg: self.task_status_callback('go_around', msg)),
            'hurdles': rospy.Subscriber('/hurdles/status', String, 
                                      lambda msg: self.task_status_callback('hurdles', msg)),
            'stairs': rospy.Subscriber('/stairs/status', String, 
                                     lambda msg: self.task_status_callback('stairs', msg)),
            'throw': rospy.Subscriber('/throw/status', String, 
                                    lambda msg: self.task_status_callback('throw', msg))
        }
        
        # 主控制状态发布器
        ros_config = self.config.get('ros', {})
        status_topic = ros_config.get('topics', {}).get('competition_status', '/competition/status')
        progress_topic = ros_config.get('topics', {}).get('competition_progress', '/competition/progress')
        queue_size = ros_config.get('queue_size', 1)
        
        self.competition_status_pub = rospy.Publisher(status_topic, String, queue_size=queue_size)
        self.competition_progress_pub = rospy.Publisher(progress_topic, String, queue_size=queue_size)

    def task_status_callback(self, task_name, msg):
        """任务状态回调"""
        status = msg.data
        print(f"[{task_name}] Status: {status}")
        
        # 更新任务完成状态
        if status == "completed":
            if task_name == "go_around":
                self.task_completed["red_object"] = True
                self.progress_info["completed_tasks"].append("Red Object Go Around")
            elif task_name == "hurdles":
                self.task_completed["blue_object"] = True
                self.progress_info["completed_tasks"].append("Blue Object Hurdles")
            elif task_name == "stairs":
                self.task_completed["green_object"] = True
                self.progress_info["completed_tasks"].append("Green Object Stairs")
            elif task_name == "throw":
                self.task_completed["purple_object"] = True
                self.progress_info["completed_tasks"].append("Purple Object Throw")

    def image_callback(self, msg):
        """ROS图像回调函数"""
        try:
            self.current_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr(f"Failed to convert image: {e}")

    def detect_colored_objects(self, image):
        """检测彩色物体"""
        if image is None:
            return {}
        
        detected = {}
        lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)
        
        # 计算检测区域
        roi_x_min = int(self.roi_x_min * self.image_width)
        roi_x_max = int(self.roi_x_max * self.image_width)
        roi_y_min = int(self.roi_y_min * self.image_height)
        roi_y_max = int(self.roi_y_max * self.image_height)
        
        # 获取当前需要检测的颜色
        current_task_name, current_color_needed = self.get_current_task_info()
        
        for color_name, color_range in self.color_ranges.items():
            # 过滤掉不需要显示的颜色
            if color_name in ["deep_green", "white"]:
                continue
                
            # 检查任务是否启用
            if color_name == "red" and "go_around" not in self.task_configs:
                continue
            if color_name == "blue" and "hurdles" not in self.task_configs:
                continue
            if color_name == "green" and "stairs" not in self.task_configs:
                continue
            if color_name == "purple" and "throw" not in self.task_configs:
                continue
            
            # 按顺序执行：只检测当前需要的颜色，或者显示所有颜色用于调试
            if self.debug_mode:
                # 调试模式显示所有颜色
                pass
            elif current_color_needed and color_name != current_color_needed:
                # 非调试模式下，只检测当前任务需要的颜色
                continue
            
            mask = cv2.inRange(lab, color_range["lower"], color_range["upper"])
            
            # 应用检测区域限制（红色物体跳过安全区域限制）
            if color_name != "red":
                roi_mask = np.zeros_like(mask)
                roi_mask[roi_y_min:roi_y_max, roi_x_min:roi_x_max] = 255
                mask = cv2.bitwise_and(mask, roi_mask)
            
            kernel = np.ones((5, 5), np.uint8)
            mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
            mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
            
            contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
            
            if len(contours) > 0:
                largest_contour = max(contours, key=cv2.contourArea)
                area = cv2.contourArea(largest_contour)
                
                if area > self.min_area:  # 使用配置的面积阈值
                    M = cv2.moments(largest_contour)
                    if M["m00"] != 0:
                        cx = int(M["m10"] / M["m00"])
                        cy = int(M["m01"] / M["m00"])
                        
                        # 检查是否在检测区域内（红色物体跳过安全区域检查）
                        if color_name == "red" or (roi_x_min <= cx <= roi_x_max and roi_y_min <= cy <= roi_y_max):
                            detected[color_name] = {
                                "center": (cx, cy),
                                "area": area,
                                "contour": largest_contour
                            }
        
        return detected

    def is_object_stable(self, color_name, center):
        """检查物体检测是否稳定"""
        if color_name not in self.detection_history:
            self.detection_history[color_name] = []
        
        self.detection_history[color_name].append(center)
        
        # 对红色物体使用更高的稳定性要求
        if color_name == "red":
            stability_frames = self.red_stability_frames
        else:
            stability_frames = self.detection_stability_frames
        
        # 保持最近几帧的记录
        if len(self.detection_history[color_name]) > stability_frames:
            self.detection_history[color_name].pop(0)
        
        # 检查是否有足够的稳定检测
        if len(self.detection_history[color_name]) >= stability_frames:
            # 计算位置变化
            recent_centers = self.detection_history[color_name]
            max_deviation = max([
                abs(center[0] - c[0]) + abs(center[1] - c[1]) 
                for c in recent_centers
            ])
            
            return max_deviation < self.position_tolerance  # 使用配置的位置变化阈值
        
        return False

    def is_object_past_target_line(self, center):
        """检查物体是否已越过目标线"""
        if self.current_image is None:
            return False

        # 计算目标线位置
        target_line_y = int(self.target_line_y_ratio * self.image_height)

        # 检查物体y坐标是否大于目标线
        return center[1] >= target_line_y

    def get_current_task_info(self):
        """获取当前应该执行的任务信息"""
        if self.current_task_index >= len(self.task_sequence):
            return None, None  # 所有任务都完成了
        
        return self.task_sequence[self.current_task_index]

    def is_current_task_ready(self, detected_objects):
        """检查当前任务是否准备好执行"""
        task_name, color_needed = self.get_current_task_info()
        
        if task_name is None:
            return False  # 所有任务完成
        
        # 检查任务是否启用
        if task_name not in self.task_configs:
            print(f"任务 {task_name} 已禁用，跳过")
            self.advance_to_next_task()
            return False
        
        # 如果不需要检测颜色（如投掷后序列、大模型任务），直接返回True
        if color_needed is None:
            return True
        
        # 检查是否检测到所需颜色且稳定
        if color_needed in detected_objects:
            center = detected_objects[color_needed]["center"]
            
            # 对于绿色、蓝色、紫色物体，需要检查是否越过目标线
            if color_needed in ["green", "blue"]:
                if self.is_object_stable(color_needed, center):
                    # 检查物体是否已越过目标线
                    if self.is_object_past_target_line(center):
                        print(f"检测到 {color_needed} 物体已越过目标线，准备执行 {task_name} 任务")
                        return True
                    else:
                        # 物体稳定但未越过目标线，继续巡线
                        target_line_y = int(self.target_line_y_ratio * self.image_height)
                        print(f"[DEBUG] image_height={self.image_height}, target_line_y_ratio={self.target_line_y_ratio}, 计算结果: {target_line_y}")
                        print(f"检测到稳定的 {color_needed} 物体，但未越过目标线 (当前y: {center[1]}, 目标线: {target_line_y})，继续巡线...")
                        return False
            else:
                # 对于红色物体（go_around任务），保留稳定性检查但去掉面积和安全区域限制
                if color_needed == "red":
                    area = detected_objects[color_needed]["area"]
                    center = detected_objects[color_needed]["center"]
                    
                    # 添加红色物体的检测条件
                    # 1. 物体稳定性检查（增加检查帧数）
                    if self.is_object_stable(color_needed, center):
                        # 2. 检查物体面积足够大（需要靠近才能执行）
                        # 3. 检查物体位置足够靠近图像底部（表示机器人已经接近）
                        target_y_min = int(self.image_height * self.red_min_y_ratio)
                        
                        if area >= self.red_min_area and center[1] >= target_y_min:
                            print(f"检测到稳定的红色物体，满足执行条件（面积: {area} >= {self.red_min_area}, y位置: {center[1]} >= {target_y_min}），准备执行绕行任务")
                            return True
                        else:
                            print(f"检测到红色物体但不满足条件（面积: {area}/{self.red_min_area}, y位置: {center[1]}/{target_y_min}），继续巡线...")
                            return False
                    else:
                        print(f"检测到红色物体但不够稳定，继续观察...")
                        return False
                else:
                    # 其他颜色的处理
                    if self.is_object_stable(color_needed, center):
                        return True
        
        return False

    def advance_to_next_task(self):
        """前进到下一个任务"""
        self.current_task_index += 1
        if self.current_task_index >= len(self.task_sequence):
            print("所有任务序列完成！")
            self.competition_finished = True

    def execute_current_task(self):
        """执行当前任务"""
        task_name, color_needed = self.get_current_task_info()
        
        if task_name is None:
            return False
        
        print(f"执行任务: {task_name} (索引: {self.current_task_index})")
        
        success = False
        if task_name == "go_around":
            success = self.start_go_around()
        elif task_name == "hurdles":
            success = self.start_hurdles()
        elif task_name == "stairs":
            success = self.start_stairs()
        elif task_name == "throw":
            success = self.start_throw_task()  # 启动投掷任务
        
        if success:
            # 等待任务完成
            self.wait_for_current_task_completion()
            # 完成后前进到下一个任务
            self.advance_to_next_task()
            return True
        
        return False

    def call_service(self, service_name, timeout=None):
        """调用ROS服务"""
        if timeout is None:
            timeout = self.config.get('ros', {}).get('service_timeout', 5.0)
        
        try:
            rospy.wait_for_service(service_name, timeout=timeout)
            service_proxy = rospy.ServiceProxy(service_name, Trigger)
            response = service_proxy(TriggerRequest())
            return response.success
        except rospy.ROSException as e:
            print(f"Service call failed for {service_name}: {e}")
            return False

    def start_line_following(self):
        """启动巡线任务"""
        print("Starting line following...")
        self.progress_info["current_task"] = "Line Following"
        self.progress_info["progress"] = "Following line..."
        self.current_state = "line_following"
        self.publish_competition_status("line_following_started")
        return self.call_service('/line_follow/start')

    def stop_line_following(self):
        """停止巡线任务"""
        print("Stopping line following...")
        return self.call_service('/line_follow/stop')

    def start_go_around(self):
        """启动绕行任务"""
        if "go_around" not in self.task_configs:
            print("绕行任务已禁用")
            return False
            
        print("Starting go around task...")
        self.progress_info["current_task"] = "Go Around Task"
        self.progress_info["progress"] = "Going around red object..."
        self.current_state = "go_around"
        self.stop_line_following()
        time.sleep(1)
        self.publish_competition_status("go_around_started")
        return self.call_service('/go_around/start')

    def start_hurdles(self):
        """启动跨栏任务"""
        if "hurdles" not in self.task_configs:
            print("跨栏任务已禁用")
            return False
            
        print("Starting hurdles task...")
        self.progress_info["current_task"] = "Hurdles Task"
        self.progress_info["progress"] = "Executing hurdles..."
        self.current_state = "hurdles"
        self.stop_line_following()
        time.sleep(1)
        self.publish_competition_status("hurdles_started")
        return self.call_service('/hurdles/start')

    def start_stairs(self):
        """启动台阶任务"""
        if "stairs" not in self.task_configs:
            print("台阶任务已禁用")
            return False
            
        print("Starting stairs task...")
        self.progress_info["current_task"] = "Stairs Task"
        self.progress_info["progress"] = "Executing stairs..."
        self.current_state = "stairs"
        self.stop_line_following()
        time.sleep(1)
        self.publish_competition_status("stairs_started")
        return self.call_service('/stairs/start')

    def start_throw_task(self):
        """启动投掷任务"""
        if "throw" not in self.task_configs:
            print("投掷任务已禁用")
            return False
            
        print("Starting throw task...")
        self.progress_info["current_task"] = "Throw Task"
        self.progress_info["progress"] = "Executing throw..."
        self.current_state = "throw"
        self.stop_line_following()
        time.sleep(1)
        self.publish_competition_status("throw_started")
        return self.call_service('/throw/start')





    def resume_line_following(self):
        """恢复巡线任务"""
        print("Resuming line following...")
        self.progress_info["current_task"] = "Line Following"
        self.progress_info["progress"] = "Continuing line following..."
        self.current_state = "line_following"
        self.publish_competition_status("line_following_resumed")
        return self.call_service('/line_follow/start')

    def monitor_line_following(self):
        """监控巡线过程中的物体检测"""
        while self.running and not self.competition_finished:
            try:
                # 检查是否有当前任务需要执行
                task_name, color_needed = self.get_current_task_info()
                
                if self.current_image is not None and self.current_state == "line_following":
                    # 检测彩色物体
                    detected_objects = self.detect_colored_objects(self.current_image)
                    
                    # 更新检测到的物体列表
                    self.progress_info["detected_objects"] = list(detected_objects.keys())
                    
                    # 检查当前任务是否准备好执行
                    if self.is_current_task_ready(detected_objects):
                        if task_name:
                            if color_needed:
                                print(f"检测到稳定的 {color_needed} 物体！切换到 {task_name} 任务...")
                            else:
                                print(f"准备执行 {task_name} 任务...")
                            
                            # 执行当前任务
                            if self.execute_current_task():
                                # 任务完成后处理
                                self.handle_task_completion()
                
                time.sleep(0.1)
                
            except Exception as e:
                print(f"Error in line following monitor: {e}")
                time.sleep(1)

    def handle_task_completion(self):
        """处理任务完成后的逻辑"""
        print(f"任务完成处理: current_task_index={self.current_task_index}, total_tasks={len(self.task_sequence)}")
        
        # 任务完成后，如果还有下一个任务，检查下一个任务类型
        if not self.competition_finished and self.current_task_index < len(self.task_sequence):
            # 检查下一个任务是否需要巡线
            next_task_name, next_color = self.get_current_task_info()
            print(f"下一个任务: {next_task_name}, 需要颜色检测: {next_color}")
            
            if next_task_name and next_color:  # 需要检测颜色的任务需要巡线
                print(f"下一个任务 {next_task_name} 需要巡线，恢复巡线...")
                time.sleep(2)  # 给机器人时间稳定
                self.resume_line_following()
        else:
            print("所有任务完成！比赛结束！")
            self.competition_finished = True
            self.progress_info["current_task"] = "Competition Completed"
            self.progress_info["progress"] = "All tasks completed!"
            self.publish_competition_status("competition_completed")

    def wait_for_current_task_completion(self):
        """等待当前任务完成"""
        task_name, _ = self.get_current_task_info()
        
        if task_name == "go_around":
            while not self.task_completed["red_object"] and self.running:
                time.sleep(0.5)
        elif task_name == "hurdles":
            while not self.task_completed["blue_object"] and self.running:
                time.sleep(0.5)
        elif task_name == "stairs":
            while not self.task_completed["green_object"] and self.running:
                time.sleep(0.5)
        elif task_name == "throw":
            while not self.task_completed["purple_object"] and self.running:
                time.sleep(0.5)

    def draw_progress_info(self, image):
        """在图像上绘制进度信息"""
        if image is None or not self.show_progress_info:
            return image
        
        display_image = image.copy()
        
        # 绘制半透明背景
        overlay = display_image.copy()
        cv2.rectangle(overlay, (10, 10), (480, 250), (0, 0, 0), -1)
        cv2.addWeighted(overlay, 0.7, display_image, 0.3, 0, display_image)
        
        # 绘制文本信息
        y_offset = 35
        
        # 当前任务
        cv2.putText(display_image, f"Current Task: {self.progress_info['current_task']}", 
                   (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2)
        y_offset += 25
        
        # 进度信息
        cv2.putText(display_image, f"Progress: {self.progress_info['progress']}", 
                   (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
        y_offset += 25
        
        # 目标线信息（对于需要检查目标线的任务）
        current_task_name, current_color = self.get_current_task_info()
        if current_color in ["green", "blue", "purple"]:
            target_line_y = int(self.target_line_y_ratio * self.image_height)
            line_info = f"Target Line: Y={target_line_y} ({self.target_line_y_ratio*100:.0f}% of height)"
            cv2.putText(display_image, line_info, 
                       (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
            y_offset += 20
        
        # 任务序列进度
        current_task_name, current_color = self.get_current_task_info()
        sequence_text = f"Sequence: {self.current_task_index + 1}/{len(self.task_sequence)}"
        if current_task_name:
            sequence_text += f" - {current_task_name}"
            if current_color:
                sequence_text += f" ({current_color})"
        cv2.putText(display_image, sequence_text, 
                   (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 1)
        y_offset += 25
        
        # 检测到的物体
        if self.progress_info['detected_objects']:
            detected_str = "Detected: " + ", ".join(self.progress_info['detected_objects'])
            cv2.putText(display_image, detected_str, 
                       (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
        y_offset += 25
        
        # 已完成的任务
        total_tasks = len(self.task_sequence)
        completed_tasks = self.current_task_index
        cv2.putText(display_image, f"Completed: {completed_tasks}/{total_tasks} tasks", 
                   (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
        y_offset += 20
        
        # 任务序列状态
        for i, (task_name, color) in enumerate(self.task_sequence):
            if i < self.current_task_index:
                # 已完成的任务
                status_text = f"[OK] {task_name}"
                color_rgb = (0, 255, 0)  # 绿色
            elif i == self.current_task_index:
                # 当前任务
                status_text = f"[>>] {task_name}"
                color_rgb = (0, 255, 255)  # 黄色
            else:
                # 未来任务
                status_text = f"[ ] {task_name}"
                color_rgb = (128, 128, 128)  # 灰色
            
            cv2.putText(display_image, status_text, 
                       (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.4, color_rgb, 1)
            y_offset += 16
        
        # 调试信息
        if self.debug_mode:
            cv2.putText(display_image, f"Debug: ROI({self.roi_x_min:.1f},{self.roi_y_min:.1f})-({self.roi_x_max:.1f},{self.roi_y_max:.1f})", 
                       (20, y_offset), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 0), 1)
            y_offset += 18
        
        # 比赛状态
        if self.competition_finished:
            cv2.putText(display_image, "COMPETITION COMPLETED!", 
                       (20, y_offset + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
        
        return display_image

    def draw_detected_objects(self, image, detected_objects):
        """在图像上标注检测到的物体"""
        if image is None:
            return image
        
        display_image = image.copy()
        
        # 绘制检测区域
        if self.debug_mode:
            roi_x_min = int(self.roi_x_min * self.image_width)
            roi_x_max = int(self.roi_x_max * self.image_width)
            roi_y_min = int(self.roi_y_min * self.image_height)
            roi_y_max = int(self.roi_y_max * self.image_height)
            
            cv2.rectangle(display_image, (roi_x_min, roi_y_min), (roi_x_max, roi_y_max), (255, 255, 0), 2)
            cv2.putText(display_image, "Detection ROI", (roi_x_min, roi_y_min - 10), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 1)
        
        # 绘制目标线（对于绿色、蓝色、紫色物体）
        current_task_name, current_color_needed = self.get_current_task_info()
        if current_color_needed in ["green", "blue", "purple"]:
            target_line_y = int(self.target_line_y_ratio * self.image_height)
            
            # 绘制目标线（横跨整个画面宽度）
            cv2.line(display_image, (0, target_line_y), (self.image_width, target_line_y), (0, 255, 255), 3)
            cv2.putText(display_image, f"Target Line ({current_color_needed})", (10, target_line_y - 10), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
            
            # 在右端添加箭头指示（物体需要越过此线）
            arrow_start_x = self.image_width - 50
            arrow_start_y = target_line_y - 20
            arrow_end_x = self.image_width - 50
            arrow_end_y = target_line_y + 20
            cv2.arrowedLine(display_image, (arrow_start_x, arrow_start_y), (arrow_end_x, arrow_end_y), (0, 255, 255), 2)
            cv2.putText(display_image, "CROSS", (self.image_width - 80, target_line_y + 35), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 255), 1)
        
        if not detected_objects:
            return display_image
        
        color_map = {
            "red": (0, 0, 255),
            "blue": (255, 0, 0),
            "green": (0, 255, 0),
            "purple": (255, 0, 255)
        }
        
        # 获取当前需要检测的颜色
        current_task_name, current_color_needed = self.get_current_task_info()
        
        for color_name, obj_info in detected_objects.items():
            color = color_map.get(color_name, (255, 255, 255))
            center = obj_info["center"]
            
            # 如果是当前需要的颜色，使用更粗的线条
            line_thickness = 3 if color_name == current_color_needed else 2
            circle_radius = 10 if color_name == current_color_needed else 8
            
            # 绘制物体轮廓
            if self.show_detection_masks:
                cv2.drawContours(display_image, [obj_info["contour"]], -1, color, line_thickness)
            
            # 绘制中心点
            cv2.circle(display_image, center, circle_radius, color, -1)
            
            # 检查稳定性和位置
            is_stable = self.is_object_stable(color_name, center)
            stability_text = "STABLE" if is_stable else "UNSTABLE"
            stability_color = color if is_stable else (128, 128, 128)
            
            # 对于绿色、蓝色、紫色物体，检查是否越过目标线
            position_text = ""
            position_color = (128, 128, 128)
            if color_name in ["green", "blue", "purple"] and is_stable:
                is_past_line = self.is_object_past_target_line(center)
                if is_past_line:
                    position_text = "PAST LINE"
                    position_color = (0, 255, 0)  # 绿色
                else:
                    position_text = "APPROACHING LINE"
                    position_color = (0, 255, 255)  # 黄色
            
            # 标注物体名称和稳定性
            font_scale = 0.7 if color_name == current_color_needed else 0.6
            cv2.putText(display_image, f"{color_name.upper()}", 
                       (center[0] - 40, center[1] - 30), 
                       cv2.FONT_HERSHEY_SIMPLEX, font_scale, color, 2)
            cv2.putText(display_image, stability_text, 
                       (center[0] - 40, center[1] - 10), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.4, stability_color, 1)
            
            # 显示位置状态（对于绿色、蓝色、紫色物体）
            if position_text:
                cv2.putText(display_image, position_text, 
                           (center[0] - 40, center[1] + 10), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.4, position_color, 1)
            
            # 如果是当前需要的颜色，添加特殊标记
            if color_name == current_color_needed:
                target_text = "TARGET"
                if color_name in ["green", "blue", "purple"] and position_text == "PAST LINE":
                    target_text = "READY!"
                cv2.putText(display_image, target_text, 
                           (center[0] - 30, center[1] + 25), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 255), 2)
        
        return display_image

    def publish_competition_status(self, status):
        """发布比赛状态"""
        msg = String()
        msg.data = status
        self.competition_status_pub.publish(msg)
        
        progress_msg = String()
        progress_msg.data = f"Task: {self.progress_info['current_task']}, Progress: {self.progress_info['progress']}"
        self.competition_progress_pub.publish(progress_msg)

    def set_servo(self, pan, tilt):
        """设置舵机位置"""
        try:
            self.motion_manager.set_servos_position(self.servo_time, 
                                                  [[self.head_pan_id, pan], [self.head_tilt_id, tilt]])
        except Exception as e:
            rospy.logerr(f"Servo control error: {e}")

    def shutdown(self, signum, frame):
        """节点关闭回调函数"""
        print("Shutting down competition controller...")
        with self.lock:
            self.running = False
            
            # 停止所有任务
            for service_name in ['/line_follow/stop', '/go_around/stop', '/hurdles/stop', 
                               '/stairs/stop', '/throw/stop']:
                try:
                    self.call_service(service_name, timeout=2.0)
                except:
                    pass
            
            self.motion_manager.run_action('stand')
            self.publish_competition_status("shutdown")
            common.loginfo('%s shutdown' % self.name)

    def run(self):
        """主运行函数"""
        print("Starting competition...")
        
        # 等待图像数据
        while self.current_image is None and self.running:
            rospy.sleep(0.1)
        
        print("Camera ready, starting competition sequence...")
        
        # 先停止可能已经运行的循线任务
        print("Stopping any existing line following...")
        self.stop_line_following()
        time.sleep(1)
        
        # 先执行固定前进五步
        if self.current_state == "forward_start":
            print("Executing initial forward walk...")
            self.execute_forward_steps()
            
            # 前进完成后切换到循线状态
            self.current_state = "line_following"
            time.sleep(1)  # 给机器人时间稳定
        
        # 开始巡线任务
        print("Starting line following after forward walk...")
        if self.start_line_following():
            # 在单独线程中监控巡线过程
            monitor_thread = threading.Thread(target=self.monitor_line_following)
            monitor_thread.daemon = True
            monitor_thread.start()
        
        # 主显示循环
        while self.running and not rospy.is_shutdown():
            try:
                if self.current_image is not None:
                    # 检测物体（用于显示）
                    detected_objects = self.detect_colored_objects(self.current_image)
                    
                    # 创建显示图像
                    display_image = self.current_image.copy()
                    
                    # 标注检测到的物体
                    display_image = self.draw_detected_objects(display_image, detected_objects)
                    
                    # 绘制进度信息
                    display_image = self.draw_progress_info(display_image)
                    
                    # 根据配置决定是否显示主窗口
                    # if self.show_main_window:
                    #     # 应用窗口缩放
                    #     if self.window_scale != 1.0:
                    #         new_width = int(display_image.shape[1] * self.window_scale)
                    #         new_height = int(display_image.shape[0] * self.window_scale)
                    #         display_image = cv2.resize(display_image, (new_width, new_height))
                        
                    #     cv2.imshow('Competition Controller - Main Control View', display_image)
                        
                    #     # 检查退出键
                    #     key = cv2.waitKey(30) & 0xFF
                    #     if key == 27:  # ESC键
                    #         break
                    
                    # 检查是否所有任务完成
                    if self.competition_finished:
                        print("Competition completed successfully!")
                        time.sleep(5)  # 显示完成信息5秒
                        break
                        
                else:
                    rospy.sleep(0.1)
                    
            except KeyboardInterrupt:
                break
            except Exception as e:
                print(f"Error in main loop: {e}")
                rospy.sleep(0.1)
        
        # 退出前清理
        if self.show_main_window:
            cv2.destroyAllWindows()
        rospy.signal_shutdown('Competition finished')

if __name__ == '__main__':
    try:
        controller = CompetitionController('competition_controller')
        controller.run()
    except Exception as e:
        print(f"Failed to start competition: {e}")
        rospy.logerr(f"Competition initialization error: {e}")
